# AprilCalib log 1
# CalibRig::mode=2d
# @ Mon Nov 25 12:08:07 2019

from numpy import array
U=array([[330.6310424804688, 282.7915954589844, 236.4543914794922, 423.4015502929688, 465.5762023925781, 504.3968505859375, 192.3064117431641, 377.8060913085938, 381.7456970214844, 332.49365234375, 472.6939392089844, 282.951171875, 428.8907470703125, 234.8314514160156, 189.1918792724609, 512.3943481445313, 433.8626708984375, 478.7119445800781, 385.3198852539063, 334.5860595703125, 283.5167846679688, 233.7518615722656, 186.9996337890625, 519.1614990234375, 523.9880981445313, 483.0347595214844, 437.5261840820313, 388.0799255371094, 336.4168090820313, 284.2307739257813, 233.4927520751953, 185.8952178955078, 526.4038696289063, 485.4091796875, 439.5995788574219, 389.7626953125, 337.6912536621094, 285.0511474609375, 234.0546722412109, 186.0581207275391],
       [130.2817840576172, 132.7966613769531, 136.2619171142578, 128.8092803955078, 129.7339477539063, 131.8710327148438, 141.2012481689453, 128.9645843505859, 172.4571533203125, 173.8562774658203, 171.6019287109375, 176.2408905029297, 171.5320281982422, 179.5469360351563, 183.2366333007813, 172.5430297851563, 218.4861755371094, 217.60400390625, 219.9787750244141, 221.8102569580078, 224.0023803710938, 226.5166625976563, 229.0492401123047, 216.7470092773438, 263.323974609375, 265.6042175292969, 267.9904174804688, 270.4181518554688, 272.4883117675781, 274.5068969726563, 276.1083374023438, 277.3498840332031, 310.7899169921875, 315.0412902832031, 318.9205017089844, 322.1263122558594, 324.6394958496094, 326.2000427246094, 326.8544311523438, 326.3442993164063]], dtype='float64');
Xw=array([[699.5044555664063, 499.5044555664063, 299.5044555664063, 1099.504516601563, 1299.504516601563, 1499.504516601563, 99.50446319580078, 899.5044555664063, 899.5044555664063, 699.5044555664063, 1299.504516601563, 499.5044555664063, 1099.504516601563, 299.5044555664063, 99.50446319580078, 1499.504516601563, 1099.504516601563, 1299.504516601563, 899.5044555664063, 699.5044555664063, 499.5044555664063, 299.5044555664063, 99.50446319580078, 1499.504516601563, 1499.504516601563, 1299.504516601563, 1099.504516601563, 899.5044555664063, 699.5044555664063, 499.5044555664063, 299.5044555664063, 99.50446319580078, 1499.504516601563, 1299.504516601563, 1099.504516601563, 899.5044555664063, 699.5044555664063, 499.5044555664063, 299.5044555664063, 99.50446319580078],
       [99.50446319580078, 99.50446319580078, 99.50446319580078, 99.50446319580078, 99.50446319580078, 99.50446319580078, 99.50446319580078, 99.50446319580078, 299.5044555664063, 299.5044555664063, 299.5044555664063, 299.5044555664063, 299.5044555664063, 299.5044555664063, 299.5044555664063, 299.5044555664063, 499.5044555664063, 499.5044555664063, 499.5044555664063, 499.5044555664063, 499.5044555664063, 499.5044555664063, 499.5044555664063, 499.5044555664063, 699.5044555664063, 699.5044555664063, 699.5044555664063, 699.5044555664063, 699.5044555664063, 699.5044555664063, 699.5044555664063, 699.5044555664063, 899.5044555664063, 899.5044555664063, 899.5044555664063, 899.5044555664063, 899.5044555664063, 899.5044555664063, 899.5044555664063, 899.5044555664063],
       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]], dtype='float64');
# After LM:
K=array([[444.2781133769743, 0, 317.2221681196752],
       [0, 443.7287490647113, 263.2608251055756],
       [0, 0, 1]], dtype='float64');
distCoeffs=array([-0.4123817982621272,
       0.135366917884488,
       -0.009792861283491503,
       -0.01251905763438609,
       0], dtype='float64');
CovK=array([[5.345522444311048, 5.49406756837365, 0.139508285051919, 0.3323183348053564, -0.009050582254400079, 0.004190055698497924, -0.0002132055180840496, -0.0002049698862342004, 0.002132110780316418],
       [5.494067568373301, 5.650324066412051, 0.1540071928944101, 0.3371324727945108, -0.009276582027031435, 0.004236141200584105, -0.000213615934103696, -0.0002155112528075509, 0.002247520180918916],
       [0.1395082850438044, 0.1540071928860715, 0.2275085396230987, -0.009393918687217808, -0.0003640675268313664, 0.0007209547601680274, 2.690889272910182e-05, -0.0001045505673315688, -0.0008428509584129441],
       [0.3323183348050138, 0.33713247279417, -0.009393918686924109, 0.1971230662354849, -0.0005698961269606887, 7.685971752089236e-05, -0.0001072241293821852, -2.002717168486855e-05, 0.0001770567616568356],
       [-0.009050582254394085, -0.009276582027025861, -0.0003640675268444676, -0.0005698961269608454, 2.032163582192536e-05, -2.651005532672106e-05, 3.679569759420252e-07, 3.955082757068279e-07, 1.710749306060611e-05],
       [0.004190055698471371, 0.004236141200556836, 0.0007209547601720223, 7.685971751937188e-05, -2.651005532668066e-05, 8.592403735354783e-05, 3.153808960945491e-08, -4.685458021975536e-07, -9.04828440406363e-05],
       [-0.0002132055180847277, -0.0002136159341044139, 2.690889272887316e-05, -0.0001072241293822247, 3.679569759433768e-07, 3.153808960801146e-08, 7.525056676397375e-08, 1.718353433443302e-09, -2.486560888248169e-07],
       [-0.0002049698862305899, -0.0002155112528038474, -0.0001045505673317699, -2.002717168475442e-05, 3.95508275701167e-07, -4.685458021964772e-07, 1.718353433329444e-09, 6.383686423679283e-08, 4.622745824877945e-07],
       [0.002132110780353749, 0.002247520180957878, -0.000842850958406721, 0.000177056761659181, 1.710749306054088e-05, -9.048284404059697e-05, -2.486560888256863e-07, 4.622745824840638e-07, 0.0001074973071097885]], dtype='float64');
# rms=0.236002
r0=array([0.1760011295722216,
       0.1624294099673488,
       -1.951889985139565], dtype='float64');
t0=array([-641.6885817087174,
       655.5154064631812,
       2065.349650856974], dtype='float64');
Covr0=array([[1.926761618829216e-06, 9.295619901894297e-07, 2.12575319674819e-07],
       [9.295619901968624e-07, 1.944038746623312e-06, 1.91082889883015e-07],
       [2.125753196746739e-07, 1.910828898818235e-07, 4.615841670208643e-08]], dtype='float64');
Covt0=array([[4.862719044152833, 0.006917512758034546, -2.976207521715677],
       [0.006917512766523151, 4.282979708119403, -9.721338498097275],
       [-2.976207521892926, -9.721338498107427, 120.1222130119242]], dtype='float64');
r1=array([-0.1712548586749414,
       0.07486055370833834,
       -0.03211563781067454], dtype='float64');
t1=array([-642.9236483254732,
       -626.6517638065574,
       1861.479948683382], dtype='float64');
Covr1=array([[9.213172867713157e-07, -9.64435884557526e-08, -1.523185517071482e-08],
       [-9.644358845910344e-08, 6.791967506944827e-07, 9.834712003642615e-08],
       [-1.523185517137688e-08, 9.834712003654763e-08, 1.973373348147665e-08]], dtype='float64');
Covt1=array([[3.943850393244386, -0.2892822135541237, -1.107419796495476],
       [-0.2892822135508756, 3.12769635667903, -4.058824745255932],
       [-1.10741979663531, -4.058824745261305, 89.82176846048205]], dtype='float64');
